#include "planning_adaptor.h"

//函数为测试使用
void generate_random_path_points(PlanningPath &path, const size_t N, const double max_range = 10)
{
    std::cout << "Generating "<< N << " path cloud...";
    auto_msgs::planning_point temp_point;            
    for (size_t i = 0; i < N;i++)
    {
        temp_point.x = max_range * (rand() % 1000) / 1000.0;
        temp_point.y = max_range * (rand() % 1000) / 1000.0;
        temp_point.theta = max_range * (rand() % 1000) / 1000.0;
        path.path_.path.push_back(temp_point);
    }
    std::cout << "done\n";
}

void build_kb_tree(planning_kd_tree_t & index)
{
	

}

int main()
{   
    PlanningPath path;
    generate_random_path_points(path,10000);
    path.write_path_to_csv("rtk_plannig.csv");
    path.read_csv_to_path("rtk_plannig.csv");
    path.write_path_to_csv("rtk_plannig2.csv");
    clock_t startTime,endTime;

    // construct a kd-tree index:
    startTime = clock();//计时开始
    PlanningPath2KD  planning_path2kd(path); // The adaptor
    planning_kd_tree_t   index(2 /*dim*/, planning_path2kd,
                             KDTreeSingleIndexAdaptorParams(10 /* max leaf */) );//tree   
    index.buildIndex();
    dump_mem_usage();
    endTime = clock();//计时结束

    std::cout << "The tree build time is: " <<(double)(endTime - startTime) 
            / CLOCKS_PER_SEC << "s" << std::endl;
    
    double query_pt[2] = { 0, 0 };

	
		

	// do a knn search
    startTime = clock();//计时开始
	const size_t num_results = 1;
	size_t ret_index;
	double out_dist_sqr;
	nanoflann::KNNResultSet<double> resultSet(num_results);
	resultSet.init(&ret_index, &out_dist_sqr );
	index.findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10));
	//index.knnSearch(query, indices, dists, num_results, mrpt_flann::SearchParams(10));
	std::cout << "knnSearch(nn="<<num_results<<"): \n";
	std::cout << "ret_index=" << ret_index << " out_dist_sqr=" << out_dist_sqr <<std::endl;
    endTime = clock();//计时结束

    std::cout << "The tree search time is: " <<(double)(endTime - startTime) 
            / CLOCKS_PER_SEC << "s" << std::endl;
}